// system
#include <stdio.h>
#include <math.h>
// event
#include "event_int.h"
#include "lane_detection.h"
#include "lane_manager.h"

/**
*  @b Description
*  @n
*     向滑动窗口计算器中添加数据
*  @param[in] ma
*		一个指向滑动窗口计算器的指针
*  @param[in] newData
*     待添加的数据
*  @retval
*      None
*/
void add_data(WindowsInfo* ma, float newData)
{
	if (ma->valid_count < DETECT_WINDOW_SIZE)
	{
		// 数组未满：追加数据并更新总和
		ma->buffer[ma->write_pos] = newData;
		ma->sum += newData;
		ma->valid_count++;
	}
	else
	{
		// 数组已满：替换旧数据并更新总和
		ma->sum -= ma->buffer[ma->write_pos];  // 移除旧数据
		ma->sum += newData;                // 加入新数据
		ma->buffer[ma->write_pos] = newData;   // 覆盖旧值
	}
	// 循环索引
	ma->read_pos = ma->write_pos;
	ma->write_pos = (ma->write_pos + 1) % DETECT_WINDOW_SIZE;
}

/**
*  @b Description
*  @n
*     获取滑动窗口计算器中的当前数据
*  @param[in] ma
*		一个指向滑动窗口计算器的指针
*  @retval
*      None
*/
float getCurrentData(WindowsInfo* ma)
{
	if (ma->valid_count > 0)
		return ma->buffer[ma->read_pos];
	return -1;
}

/**
*  @b Description
*  @n
*     初始化滑动窗口计算器
*  @param[in] ma
*		一个指向滑动窗口计算器的指针
*  @retval
*      None
*/
void initData(WindowsInfo* ma)
{
	ma->sum = 0;
	ma->read_pos = 0;
	ma->valid_count = 0;
	ma->write_pos = 0;
}

/**
*  @b Description
*  @n
*     初始化事件检测的历史信息
*  @param[in] his
*		一个指向历史信息的指针
*  @retval
*      None
*/
void initHistorydata(HistoryInfo* his)
{
	his->retrogradeNum = 0;
	his->overSpeedingNum = 0;
	his->stopNum = 0;
	his->underSpeedingNum = 0;
	his->changeLaneNum = 0;
	initData(&his->vel_x);
	initData(&his->vel_y);
}

#pragma region 资源分配
void* event_unitCreate(RoadTopology* roadParam, EventConfiguration* param)
{
	DetectionUnitInstance* inst;
	inst = (DetectionUnitInstance*)gtrack_alloc(1, sizeof(DetectionUnitInstance));
	if (inst != NULL)
	{
		inst->cur_laneID = 0;
		inst->historyInfo.vel_x.write_pos = 0;
		inst->historyInfo.vel_y.write_pos = 0;
		inst->historyInfo.vel_x.read_pos = 0;
		inst->historyInfo.vel_y.read_pos = 0;
		inst->roadParam = roadParam;
		inst->thresholdParam = param;
		inst->state = NotDetecting;
	}
	else
	{
		event_unitDelete(inst);
		inst = NULL;
	}
	return inst;
}

void event_unitStart(void* handle)
{
	DetectionUnitInstance* inst;
	inst = (DetectionUnitInstance*)handle;
	initHistorydata(&inst->historyInfo);
}

void event_unitDelete(void* handle)
{
	gtrack_free(handle,sizeof(DetectionUnitInstance));
}

void event_unitStop(void* handle)
{
	DetectionUnitInstance* inst;
	inst = (DetectionUnitInstance*)handle;
	inst->state = NotDetecting;
}

void event_creatRoad(void* handle, event_moduleConfig* config)
{
	DetectionModuleInstance* inst;
	inst = (DetectionModuleInstance*)handle;
	//车道信息生成
	inst->roadParam = (RoadTopology*)gtrack_alloc(1, sizeof(RoadTopology));//给车道参数预分配空间
	uint8_t numCurve = config->laneParams.numControlPoints / 2;
	if (numCurve > EVENT_MAX_CONTROL_POINTS / 2)
		numCurve = EVENT_MAX_CONTROL_POINTS / 2;
	float width = config->laneParams.width;
	InsertDirection direction = config->laneParams.direction;
	BezierCurve* basecurve = (BezierCurve*)gtrack_alloc(numCurve, sizeof(BezierCurve));
	{
		uint8_t id = 0;
		for (uint8_t p = 0; p < numCurve; p++)
		{
			basecurve[p].p0 = config->laneParams.ControlPoints[id++];
			basecurve[p].p1 = config->laneParams.ControlPoints[id++];
			basecurve[p].p2 = config->laneParams.ControlPoints[id];
		}
	}
	road_add_lane(inst->roadParam, config->laneParams.maxNumLanes, basecurve, numCurve, width, direction);
	gtrack_free(basecurve, numCurve*(sizeof(BezierCurve)));
}
#pragma endregion

#pragma region 具体判定函数
int event_unitPrepross(DetectionUnitInstance* inst, GTRACK_targetDesc* t)
{
	Point2D cur_pos = {t->S[0],t->S[1]};
	int isNewTarGet = 0;

	inst->cur_laneID = detect_current_lane(inst->roadParam, &cur_pos);
	if (inst->state == NotDetecting)
	{
		//没被检测过且被判定在车道区域内，开启检测，并认为是一个新增目标
		if (inst->cur_laneID != 0)
		{
			event_unitStart(inst);
			isNewTarGet = 1;
		}
	}
	//add_data(&inst->historyInfo.pos_x, t->S[0]);//x 坐标
	//add_data(&inst->historyInfo.pos_y, t->S[1]);//y 坐标
	add_data(&inst->historyInfo.vel_x, t->S[2]);//x 方向速度
	add_data(&inst->historyInfo.vel_y, t->S[3]);//y 方向速度
	return isNewTarGet;
}

TrafficEventType event_unitLaneChangeDetection(void* handle)
{
	DetectionUnitInstance* inst;
	inst = (DetectionUnitInstance*)handle;
	uint16_t l_iConfirm_laneChange_ThresHold = inst->thresholdParam->paramsThr.lane_change.duration_stamp;

	if (inst->cur_laneID != inst->last_laneID) //
	{
		inst->historyInfo.changeLaneNum++;	
	}
	else
	{
		inst->historyInfo.changeLaneNum = 0;
	}

	TrafficEventType isIllegalLaneChange = EVENT_NONE;
	if (inst->historyInfo.changeLaneNum > l_iConfirm_laneChange_ThresHold)//意味着发生了变道
	{
		laneType detected_lane_type = SOLIDLINE;
		
		if (inst->cur_laneID + 1 == inst->last_laneID)// 从大车道-> 小车道
		{
			detected_lane_type = inst->thresholdParam->parameRoad.type[inst->cur_laneID-1];
		}
		else if (inst->last_laneID + 1 == inst->cur_laneID) //从小车道->大车道
		{
			detected_lane_type = inst->thresholdParam->parameRoad.type[inst->last_laneID-1];
		}
		// 特殊情况处理，相邻帧不在相邻车道的处理？
		else
		{
			//...
		}

		if (detected_lane_type == SOLIDLINE)
		{
			isIllegalLaneChange = EVENT_ILLEGAL_LANE_CHANGE;//非法变道
			//inst->historyInfo.changeLaneNum = 0; //也许不用清零
		}

		/// ? 考虑上报帧数
		inst->last_laneID = inst->cur_laneID; //确认变道，修改历史所属车道
	}
	return isIllegalLaneChange;
}

TrafficEventType event_unitRetrogradeDetection(void* handle)
{
	DetectionUnitInstance* inst;
	inst = (DetectionUnitInstance*)handle;
	uint16_t l_iConfirm_Retrograde_ThresHold = inst->thresholdParam->paramsThr.reversed_driving.duration_stamp;
	if ((inst->historyInfo.vel_y.buffer[inst->historyInfo.vel_y.write_pos] > 0 && inst->thresholdParam->parameRoad.moveDirection[inst->cur_laneID-1] == MOVING_TOWARD) || (inst->historyInfo.vel_y.buffer[inst->historyInfo.vel_y.write_pos] < 0 && inst->thresholdParam->parameRoad.moveDirection[inst->cur_laneID - 1] == MOVING_AWAY))
	{
		inst->historyInfo.retrogradeNum++;
	}
	else
	{
		inst->historyInfo.retrogradeNum = 0;
	}

	if (inst->historyInfo.retrogradeNum > l_iConfirm_Retrograde_ThresHold)
	{
		//这里考虑不清零 持续报警
		return EVENT_REVERSE_DRIVING;
	}
	return EVENT_NONE;
}

TrafficEventType event_unitSpeedDetection(void* handle)
{
	//这里需要改 求速度可能需要对均值等进行分析
	DetectionUnitInstance* inst;
	inst = (DetectionUnitInstance*)handle;
	float cur_Vel = fabsf(inst->historyInfo.vel_y.buffer[inst->historyInfo.vel_y.read_pos]);
	uint8_t cur_laneID = inst->cur_laneID;

	float l_fOverSpeedThrehold = inst->thresholdParam->parameRoad.max_kph[cur_laneID - 1] * inst->thresholdParam->paramsThr.speed.detect_Threshold.upper_multiplier;
	float l_fUnderSpeedThrehold = inst->thresholdParam->parameRoad.min_kph[cur_laneID - 1] * inst->thresholdParam->paramsThr.speed.detect_Threshold.lower_multiplier;
	float l_fStopSpeedThrehold = inst->thresholdParam->paramsThr.speed.absolute_min_speed;

	uint16_t l_iConfirmSpeed_ThresHold = inst->thresholdParam->paramsThr.speed.detect_Threshold.duration_stamp;
	if(cur_Vel > l_fOverSpeedThrehold)
	{
		inst->historyInfo.stopNum = 0;
		inst->historyInfo.underSpeedingNum = 0;
		inst->historyInfo.overSpeedingNum++;
		if (inst->historyInfo.overSpeedingNum > l_iConfirmSpeed_ThresHold)
		{
			//inst->historyInfo.overSpeedingNum = 0;
			return EVENT_OVERSPEED;
		}
	}
	else if (cur_Vel < l_fUnderSpeedThrehold)
	{
		inst->historyInfo.overSpeedingNum = 0; //低速了  清零超速计数
		if (cur_Vel < l_fStopSpeedThrehold)
		{
			inst->historyInfo.stopNum++;
			if (inst->historyInfo.stopNum > l_iConfirmSpeed_ThresHold)
			{
				//inst->historyInfo.stopNum = 0;
				return EVENT_ABRUPT_STOP;
			}
		}
		else
		{
			inst->historyInfo.stopNum = 0; //有速度 清零停止计数
			inst->historyInfo.underSpeedingNum++;
			if (inst->historyInfo.underSpeedingNum > l_iConfirmSpeed_ThresHold)
			{
				//inst->historyInfo.underSpeedingNum = 0;
				return EVENT_UNDERSPEED;
			}
		}
	}
	else
	{
		// 无事发生 清零		
		inst->historyInfo.overSpeedingNum = 0;
		inst->historyInfo.stopNum = 0;
		inst->historyInfo.underSpeedingNum = 0;
	}
	return EVENT_NONE;
}
#pragma endregion